#include "aim_auto/AimAuto.hpp"
#include <omp.h>

const int NULLPOS = -1;
const int debug_delay = 60;

AimAuto::AimAuto(ros::NodeHandle& nh){
    _is_enemy_blue  = readParam<int>(nh, "/init_color_blue") > 0 ? true : false;
    _cut_ratio = readParam<float>(nh,"/cut_ratio");
    int use_pred    = readParam<int>(nh, "/use_prediction");
    int use_anti = readParam<int>(nh, "/use_antispin");
    int hit_tower_mode = readParam<int>(nh, "/hit_tower_mode");
    int thresh_low  = _is_enemy_blue ? readParam<int>(nh, "/blue_thresh_low") : readParam<int>(nh, "/red_thresh_low");
    int filter      = _is_enemy_blue ? readParam<int>(nh, "/blue_filter") : readParam<int>(nh, "/red_filter");

    double cx = readParam<double>(nh, "/cx");
    double cy = readParam<double>(nh, "/cy");
    double fx = readParam<double>(nh, "/fx");
    double fy = readParam<double>(nh, "/fy");

    cv::Mat _K = (cv::Mat_<double>(3, 3) << 
        fx, 0, cx,   0, fy, cy,    0,  0,  1
    );

    float k1 = readParam<float>(nh, "/k1");
    float k2 = readParam<float>(nh, "/k2");
    float p1 = readParam<float>(nh, "/p1");
    float p2 = readParam<float>(nh, "/p2");

    std::vector<float> _dist = {k1, k2, p1, p2};
    pos_getter.setIntrinsic(_K, _dist);

    if(!_is_enemy_blue)
        match.setEnemyColor(false, thresh_low, filter);               //敌人颜色设为红色
    else
        match.setEnemyColor(true, thresh_low, filter);                //敌人颜色设为蓝色

    match.setCutRatio(_cut_ratio);

    bool enable_hsv = readParam<int>(nh,"/hsv_threshold") > 0 ? true : false;
    //读取hsv参数
    if(_is_enemy_blue){
        int l_hmin = readParam<int>(nh,"/bl_hmin"),l_hmax = readParam<int>(nh,"/bl_hmax");
        int h_hmin = readParam<int>(nh,"/bh_hmin"),h_hmax = readParam<int>(nh,"/bh_hmax");
        int smin = readParam<int>(nh,"/b_smin"),smax = readParam<int>(nh,"/b_smax");
        int vmin = readParam<int>(nh,"/b_vmin"),vmax = readParam<int>(nh,"/b_vmax");
        match.setHsvThreshold(enable_hsv,l_hmin,l_hmax,h_hmin,h_hmax,smin,smax,vmin,vmax);
    }else{
        int l_hmin = readParam<int>(nh,"/rl_hmin"),l_hmax = readParam<int>(nh,"/rl_hmax");
        int h_hmin = readParam<int>(nh,"/rh_hmin"),h_hmax = readParam<int>(nh,"/rh_hmax");
        int smin = readParam<int>(nh,"/r_smin"),smax = readParam<int>(nh,"/r_smax");
        int vmin = readParam<int>(nh,"/r_vmin"),vmax = readParam<int>(nh,"/r_vmax");
        match.setHsvThreshold(enable_hsv,l_hmin,l_hmax,h_hmin,h_hmax,smin,smax,vmin,vmax);
    }

    hit_tower = (hit_tower_mode > 0);
    use_antispin = (use_anti > 0); 
    use_prediction = (use_pred > 0);
    aimp.initWithROS(nh);
    classifier.initWithRos(nh);
    tower_hitter.initWithROS(nh);
    K_vision << 1776.67168581218, 0, 720,
        0, 1778.59375346543, 540,
        0, 0, 1;
    K_camera << fx, 0, cx, 0,   0, fy, cy, 0,   0, 0, 1, 0;
    
    Rinv.setIdentity();
    t.setZero();

    #ifdef _BASE_DEBUGGER
        printf("AimAuto is initialized.\n");
        if(!_blue) printf("Match is set to be: enemy is red.\n");
        else printf("Match is set to be: enemy is blue.\n");
    #endif // BASE_DEBUGGER
    #ifdef DISPLAY_TARGETS
        _delay = 1;                 //按下e键之后，会以_delay ms每帧的方式进行播放(并修改slow_judge)
        slow_judge = false;         //为true时，会以600ms每帧的方式播放
    #endif // DISPLAY_TARGETS
    optimal         = NULLPOS;
    last_match      = 0;
}

AimAuto::~AimAuto(){
    ;
}

void AimAuto::reset() {
    optimal         = NULLPOS;
    optimal_carNumber = NULLPOS;
    last_match      = 0;
    no_target_cnt   = 0;
    pos_getter.reset();                 // 滑动平均滤波器重置    
    aimp.reset();                       // 预测器重置
    history_tar.clear();
    tower_hitter.reset();
}


//==========================主要的逻辑框架=========================//
//========================远距离打击自瞄集成========================//
    
void AimAuto::aimAuto(cv::Mat &src, serial_com::comm &msg){
    match.saveImg(src);
    match.findPossible();
    amp.matchAll(match.matches, match.possibles, tar_list);//查找匹配灯条
    if(tar_list.size()){                        //只有识别到了装甲板才会继续执行
        // std::cout << tar_list.size() << std::endl;
        no_target_cnt = 0;
        pos_getter.batchProcess(tar_list);
        //数字识别
        cv::Mat gray_img;
        cv::cvtColor(src, gray_img, cv::COLOR_BGR2GRAY);
        for(size_t i = 0; i < tar_list.size() ;i++) {
            tar_list[i].armor_number = -1;
        }
        aimRecognition(gray_img);
        if(!hit_tower){
            TargetDecisionByCenter(msg);
        }
        else{
            hitSpinTower(msg);
        }
    }
    else{       // 无目标零保护
        msg.x = 0.0;
        msg.y = 0.0;
        msg.z = 0.0;
        msg.status = 0x00;
        no_target_cnt ++;
        aimp.updateTime();                          // 无目标的时候，需要更新时间（这样可能可以减小跳变）
        if (no_target_cnt > 16) {
            if (aimp.isInit() == true) {            // 无目标时KF需要重置
                aimp.reset();
            }
            no_target_cnt = 0;
        }
    }
    
    #ifdef DISPLAY_TARGETS
        aimDebugDisplay(src, msg);
        aimDisplay(src, msg);
        cv::imshow("disp", src);
        key = cv::waitKey(_delay);
        if(key ==' ') {
            cv::waitKey(0);
        }
        else if(key == 'e') {
            slow_judge = !slow_judge;
            if(slow_judge) _delay = 700;
            else _delay = debug_delay;
        }
    #endif  //DISPLAY_TARGETS
    optimal = NULLPOS;                    // 最佳装甲板不保存其值
}

void AimAuto::TargetDecisionByCenter(serial_com::comm &msg) {
    bool TargetDecided = false;    // 根据msg.status判断是否处于操作手决策模式
    if(TargetDecided)  { 
        if(optimal_carNumber == -1 || !findOptimalCar()) {   //还没有选定的车 或 画面中找不到上次选定的车
            std::cout << "Car not decided || selected car not within range." << std::endl;
            PickTarget();                                 //选择离中心点最近的装甲板所属的车 作为目标车
        }
        if(findOptimalCar() && optimal_carNumber != -1) {    //目标车号的装甲板被找到了
            std::cout << "Target car found.\n";
            optimal = aim_dec.Armor_decision(tar_list, optimal_carNumber);
        }
        else {
            optimal = aim_dec.Armor_decision(tar_list);//自由决策
        }
    }
    else {
        optimal = aim_dec.Armor_decision(tar_list);//自由决策
    }
    msg.status = tar_list[optimal].armor_number;
    // printf("Optimal car is %d, car_number: %d\n", optimal, msg.status);
    msg.status <<= 4;                                      // 高四位中指示了当前锁定情况
    //  ========================== 反陀螺部分 ============================
    if(use_antispin){
        aim_deps::Armor& arm_spin = tar_list[optimal];
        camera_p = Eigen::Vector3d(arm_spin.t_vec.x, arm_spin.t_vec.y, arm_spin.t_vec.z);
        anti_spin.anti(tar_list[optimal].t_vec, msg,camera_p);
        is_spin = anti_spin.spining();
        if(is_spin){
            arm_spin.t_vec.x = camera_p(0);
        }
    }   

    //  ========================== 预测部分 ============================
    if (use_prediction) {
        aim_deps::Armor& arm = tar_list[optimal];
        camera_p = Eigen::Vector3d(arm.t_vec.x, arm.t_vec.y, arm.t_vec.z);
        if (aimp.translatePredict(arm.t_vec, msg, camera_p) == true) {          // z 轴无预测
            arm.t_vec.x = camera_p(0);
            arm.t_vec.y = camera_p(1);
        }
    }
    aimBallistic(msg);//发送到串口通讯模块
    
}

void AimAuto::CalcuCenter() {
    Eigen::Vector3d Camera;
    Eigen::Vector3d Vision = Eigen::Vector3d(960,540,1);
    Eigen::Matrix4d Vision_RT;
    Eigen::Vector3d vision_cam = K_vision.inverse() * Vision * 30;
    Eigen::Vector3d tmp = Rinv * (vision_cam - t);
    Eigen::Vector4d vision_world(tmp.x(), tmp.y(), tmp.z(), 1);
    Camera = K_camera * Vision_RT.inverse() * vision_world;
    CameraCenter = cv::Point2f(Camera(0), Camera(1));
}

void AimAuto::PickTarget() {
    double min_dist = 1e9;
    double _dist;
    int _optimal = 0;
    cv::Point2f armorCenter;
    for(size_t i = 0; i < tar_list.size(); i++) {
        armorCenter = (tar_list[i].left_light.box.center + tar_list[i].right_light.box.center) / 2;
        _dist = (armorCenter.x - CameraCenter.x) * (armorCenter.x - CameraCenter.x) + (armorCenter.y - CameraCenter.y) * (armorCenter.y - CameraCenter.y);
        if(_dist < min_dist) {
            _optimal = i ;
            min_dist = _dist;
        }
    }
    optimal_carNumber = tar_list[_optimal].armor_number;
    if(!history_tar.size()){
        history_tar.push_back(optimal_carNumber);
    }else{
        std::cout<<"The number of history"<<history_tar.size()<<std::endl;
        if(history_tar.size()>50)history_tar.pop_front();
        std::vector<int> histogram(10,0);
        for( int i=0; i<history_tar.size(); ++i )
            ++histogram[ history_tar[i] ];
        int mode_carnum= std::max_element( histogram.begin(), histogram.end() ) - histogram.begin();
        for(auto tar:tar_list){
            if(tar.armor_number==mode_carnum) {
                history_tar.push_back(optimal_carNumber);
                optimal_carNumber=mode_carnum;
                return;
            }
        }
        history_tar.push_back(optimal_carNumber);
    }
}

bool AimAuto::findOptimalCar() {
    for(size_t i = 0;i < tar_list.size(); i++) {
        if(optimal_carNumber == tar_list[i].armor_number) {
            return true;
        }
    }
    return false;
}

void AimAuto::aimDebugDisplay(cv::Mat &src, const serial_com::comm & msg){
    char str[20];
	snprintf(str, 20, "Delta yaw:%f", msg.x);                 //云台转动信息yaw
	cv::putText(src, str,cv::Point(30, 360),
		cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
    snprintf(str, 20, "Delta pitch:%f", msg.y);             //云台转动信息pitch
	cv::putText(src, str,cv::Point(30, 330),
		cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
    snprintf(str, 20, "Delay: %f", msg.z);                     //子弹滞空时间
	    cv::putText(src, str,cv::Point(30, 480),
	    	cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
}

//=======================非集成模块函数=========================//
void AimAuto::aimDisplay(cv::Mat &src, const serial_com::comm &msg){
    char str[20];
    cv::line(src, cv::Point(0, aim_deps::VIDEO_HEIGHT*_cut_ratio), cv::Point(aim_deps::VIDEO_WIDTH, aim_deps::VIDEO_HEIGHT*_cut_ratio), cv::Scalar(255, 255, 0)); //画屏蔽线
    amp.drawArmorPlates(src, tar_list, optimal,is_spin);	
    if(optimal >= 0){
        snprintf(str, 20, "X: %f", tar_list[optimal].t_vec.x);      //最佳装甲板位置x
	    cv::putText(src, str,cv::Point(30, 390),
	    	cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
	    snprintf(str, 20, "Y: %f", tar_list[optimal].t_vec.y);      //最佳装甲板位置y
	    cv::putText(src, str,cv::Point(30, 420),
	    	cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
	    snprintf(str, 20, "Z: %f", tar_list[optimal].t_vec.z);      //最佳装甲板位置z
	    cv::putText(src, str,cv::Point(30, 450),
	    	cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 100, 0));
    }
    #ifdef DRAW_CONTOUR
        match.drawContour(src); 
    #endif
    match.drawLights(src);							//绘制所有灯条
    // ==================== 电控坐标系转换测试 ====================
    // Eigen::Quaterniond c2w = angle2Quat(msg.y, - msg.x, 0.0);
    // Eigen::Vector3d pw(0, 0, 10);
    // Eigen::Vector3d cam = c2w.conjugate() * pw * 1000;
    // =========================================================

    // ==================== 预测 绘制预测位置 =================
    if (tar_list.size()) {
        aim_deps::Armor& arm = tar_list[optimal];
        Eigen::Vector3d tar_pos(arm.t_vec.x, arm.t_vec.y, arm.t_vec.z);
        aimp.drawProjected(src, tar_pos, camera_p);
    }
}    

void AimAuto::aimRecognition(const cv::Mat &src){
    #pragma omp parallel for num_threads(2)
    for(size_t i = 0; i<tar_list.size(); ++i){
        if (tar_list[i].valid == false) continue; 
        classifier.reco(tar_list[i], src);
    }
}

void AimAuto::hitSpinTower(serial_com::comm &msg){
    for(int i = 0; i < tar_list.size(); i++){
        if(tar_list[i].armor_number == 7){//只选取前哨站装甲板，且应该只会存在一个
            aim_deps::Armor& tower_armor = tar_list[i];
            camera_p = Eigen::Vector3d(tower_armor.t_vec.x, tower_armor.t_vec.y, tower_armor.t_vec.z);
            tower_hitter.hit(tower_armor.t_vec,msg,camera_p);
        }
    }
    msg.x = camera_p(0);
    msg.y = camera_p(1);
    msg.z = camera_p(2);

}
void AimAuto::aimBallistic(serial_com::comm &msg){
    // printf("Aim Base x, y, z: %f, %f, %f\n", msg.x, msg.y, msg.z);
    if(tar_list.size() == 0){       // 视野内没有目标，则退出
        msg.x = 0.0;
        msg.y = 0.0;
        msg.z = 0.0;
        msg.status = aim_deps::NO_SHOOTING;
        return;
    }
    if(optimal != NULLPOS){                   //存在一个最佳决策
        /// TODO:
        cv::Point3f pos = tar_list[optimal].t_vec;

        /// 计算： 是否进行静止装甲板滤波
        pos_getter.calcBallistic(pos, msg);
        msg.x = pos.x;
        msg.y = pos.y;
        msg.z = pos.z;
    }
    else{                           // 无最佳决策
        //printf("Optimal pos is null[%d], can nnot fire.\n", optimal);
        msg.x = 0.0;
        msg.y = 0.0;
        msg.z = 0.0;
        msg.status = aim_deps::NO_SHOOTING;
    }
    // printf("After ballistics x, y, z: %f, %f, %f\n", msg.x, msg.y, msg.z);
}